#include <math.h>

#include "fdecl.h"
#include "misc.h"
#include "deformation.h"

void compute_deformation(hybroid *h)
{
  double ww1 = h->w1, denom = h->w0 + 4.0 * ww1;
  int i, spole, npole;
  int numnds = h->numnodes;
  node **south_nbrs, **nbrsi, **north_nbrs;
  node *ndei, *nds = h->nodes;
  vector *d, *f, *dsouth, *dwest, *deast, *dnorth, *fsouth, *fnorth;
  vector dsum_south, dsum_north;

  for (i = 0; i < numnds - 2; i++) {
    ndei = &nds[i];
    d = &ndei->displ;
    f = &ndei->rforce;

    nbrsi = ndei->neighbors;
    dsouth = &nbrsi[0]->displ;
    dwest  = &nbrsi[1]->displ; 
    deast  = &nbrsi[2]->displ; 
    dnorth = &nbrsi[3]->displ;
/* if the values are too big, they can be scaled by mult. with a small value */
    d->x = (ww1 * (dsouth->x + dwest->x + deast->x + dnorth->x)
	    + f->x) / denom;

    d->y = (ww1 * (dsouth->y + dwest->y + deast->y + dnorth->y)
	    + f->y) / denom;

    d->z = (ww1 * (dsouth->z + dwest->z + deast->z + dnorth->z)
	    + f->z) / denom;
  }

  /* do poles */

  spole = h->numnodes - 2; npole = h->numnodes - 1;

  south_nbrs = nds[spole].many_neighbors;
  vzeroize(dsum_south);
  for (i = 0; i < nds[spole].num_neighbors; i++) {
    vinc(south_nbrs[i]->displ, dsum_south);
  }
  vscale(ww1, dsum_south, dsum_south);
  vplus(nds[spole].rforce, dsum_south, dsum_south);
  vscale(1.0/(nds[spole].num_neighbors), dsum_south, dsum_south);
  vset(dsum_south, nds[spole].displ);

  north_nbrs = nds[npole].many_neighbors;
  vzeroize(dsum_north);
  for (i = 0; i < nds[npole].num_neighbors; i++) {
    vinc(north_nbrs[i]->displ, dsum_north);
  }
  vscale(ww1, dsum_north, dsum_north);
  vplus(nds[npole].rforce, dsum_north, dsum_north);
  vscale(1.0/(nds[npole].num_neighbors), dsum_north, dsum_north);
  vset(dsum_north, nds[npole].displ);

/* I am not updating here the current derivatives of deformation parameters */
/* since the updating of d->x etc is not done based on its previous value *
/* a possible hack would be to assume the derivative is d->x(new)-d->x(old) */
    
}


void compute_element_deformation(hybroid *h)
{


  int i, j, k;
  elem *elmi,*elms = h->elems;
  node *ndei, *nds = h->nodes;
  vector temp, temp1;
  vector elmv0, elmv1, elmv2, elmv3;
  double numelms = h->numelems, numnds = h->numnodes;
/*previously it had the value 10.0 and 1.5 for 11-bend*/
  double ddt = 1.5 * h->dt; /* Euler step */
  double par4 = 9.4, par3 = 1.41;    /* parameters for scaling */
                                    /* the deformation force  */
                                    /* due to stiffness */

  double  s400 = par4 * h->s400, s401 = par4 * h->s401;
  double  s402 = par4 * h->s402, s403 = par4 * h->s403;
  double  s411 = par4 * h->s411, s412 = par4 * h->s412;
  double  s413 = par4 * h->s413, s422 = par4 * h->s422;
  double  s423 = par4 * h->s423, s433 = par4 * h->s433;

  double  s300_1 = par3 * h->s300_1, s301_1 = par3 * h->s301_1;
  double  s302_1 = par3 * h->s302_1, s311_1 = par3 * h->s311_1;
  double  s312_1 = par3 * h->s312_1, s322_1 = par3 * h->s322_1;

  double  s300_2 = par3 * h->s300_2, s301_2 = par3 * h->s301_2;
  double  s302_2 = par3 * h->s302_2, s311_2 = par3 * h->s311_2;
  double  s312_2 = par3 * h->s312_2, s322_2 = par3 * h->s322_2;

  double  s300_3 = par3 * h->s300_3, s301_3 = par3 * h->s301_3;
  double  s302_3 = par3 * h->s302_3, s311_3 = par3 * h->s311_3;
  double  s312_3 = par3 * h->s312_3, s322_3 = par3 * h->s322_3;

  for (i = 0; i < numelms; i++) {
    elmi = &elms[i];
    switch(j = elmi->nomnodes) {
    case 4 : elmv0 = elmi->vertices[0]->displ;
             elmv1 = elmi->vertices[1]->displ;
             elmv2 = elmi->vertices[2]->displ;
             elmv3 = elmi->vertices[3]->displ;

       /* compute deformation force at the first elemental point */
             force_at_element_point_4(s400, s401, s402, s403, 0);

       /* compute deformation force at the second elemental point */
             force_at_element_point_4(s401, s411, s412, s413, 1);

       /* compute deformation force at the third elemental point */
             force_at_element_point_4(s402, s412, s422, s423, 2);

       /* compute deformation force at the fourth elemental point */
             force_at_element_point_4(s403, s413, s423, s433, 3);

             break;

    case 3 : elmv0 = elmi->vertices[0]->displ;
             elmv1 = elmi->vertices[1]->displ;
             elmv2 = elmi->vertices[2]->displ;

             switch(k = elmi->elemtype) {
             case 1 : 
               /* compute deformation force at the first elemental point */
                      force_at_element_point_3(s300_1, s301_1, s302_1, 0);

               /* compute deformation force at the second elemental point */
                      force_at_element_point_3(s301_1, s311_1, s312_1, 1);

               /* compute deformation force at the third elemental point */
                      force_at_element_point_3(s302_1, s312_1, s322_1, 2);

                      break;
				     
	     case 2 : 
               /* compute deformation force at the first elemental point */
                       force_at_element_point_3(s300_2, s301_2, s302_2, 0);

	       /* compute deformation force at the second elemental point */
	               force_at_element_point_3(s301_2, s311_2, s312_2, 1);

	       /* compute deformation force at the third elemental point */
               force_at_element_point_3(s302_2, s312_2, s322_2, 2); 

                       break;


	     case 3 : 
               /* compute deformation force at the first elemental point */
                       force_at_element_point_3(s300_3, s301_3, s302_3, 0);

	       /* compute deformation force at the second elemental point */
	               force_at_element_point_3(s301_3, s311_3, s312_3, 1);

	       /* compute deformation force at the third elemental point */
                       force_at_element_point_3(s302_3, s312_3, s322_3, 2); 

                       break;

             }
             break;
    }
  }


  for (i = 0; i < numnds; i++) {
    ndei = &nds[i];
/* update of current derivative of displacement */
    vset(ndei->rforce, ndei->ddispl);

    vscale(ddt, ndei->rforce, temp);

    vinc(temp, ndei->displ);
   }

 }


void  compute_superquadric_deformation_taper_bend(hybroid *h)
{
  int i, j, k;
  node *ndei, *nds = h->nodes;
  double forcex, forcey, forcez;
  double numnodes = h->numnodes;
  double ddt = 0.1 * h->dt;    /* Euler step */

  double sc = h->sc, a1 = h->a1, a2 = h->a2, a3 = h->a3, 
         e1 = h->e1, e2 = h->e2, tx = h->tapx, ty = h->tapy,
         b1 = h->bend1, b2 = h->bend2, b3 = h->bend3,
         sa1 = sc * a1, sa2 = sc * a2, sa3 = sc * a3, a1e1 = sc * a1 * e1, 
         a1e2 = sc * a1 * e2,
         a2e1 = sc * a2 * e1, a2e2 = sc * a2 * e2, a3e1 = sc * a3 * e1;

  double fsc = 0.0, fa1 = 0.0, fa2 = 0.0, fa3 = 0.0,
         fe1 = 0.0, fe2 = 0.0, ftx = 0.0, fty = 0.0,
         fb1 = 0.0, fb2 = 0.0, fb3 = 0.0;

  double u, v, uc, vc, cu, cv, su, sv, scucv, scusv, ssu, scu;
  double pcue1, pcue11, pcve21, pcve2, psue1, psve2, psue11, psve21,
         pcue1cve2, pcue1sve2, spsue1, spsue11, spcue1, kxsu, kysu, 
         pb13, pb123, comb, scomb, ccomb, logsu, logcu, logcv, logsv;
  double pi = 3.14159265358979323;
         
  for(i = 0; i < numnodes; i++) {
    ndei = &nds[i];
      cu = cos(ndei->uu); su = sin(ndei->uu);
    cv = cos(ndei->vv); sv = sin(ndei->vv);
    forcex = ndei->rforce.x;
    forcey = ndei->rforce.y;
    forcez = ndei->rforce.z;

    scucv = Sign(cu * cv); scusv = Sign(cu * sv); ssu = Sign(su);
    scu = Sign(cu);

    pcue1 = (cu < 0.0 || cu > 0.0) ? pow(fabs(cu), e1) : 0.0;
    pcue11 = (cu < 0.0 || cu > 0.0) ? pow(fabs(cu), e1 - 1.0) : 0.0;
    pcve2 = (cv < 0.0 || cv > 0.0) ? pow(fabs(cv), e2) : 0.0;
    pcve21 = (cv < 0.0 || cv > 0.0) ? pow(fabs(cv), e2 - 1.0) : 0.0;
    psve2 = (sv < 0.0 || sv > 0.0) ? pow(fabs(sv), e2) : 0.0;
    psve21 = (sv < 0.0 || sv > 0.0) ? pow(fabs(sv), e2 - 1.0) : 0.0;
    psue1 = (su < 0.0 || su > 0.0) ? pow(fabs(su), e1) : 0.0;
    psue11 = (su < 0.0 || su > 0.0) ? pow(fabs(su), e1 - 1.0) : 0.0;

    kxsu = tx * ssu * psue1 + 1.0;
    kysu = ty * ssu * psue1 + 1.0;
    pcue1cve2 = scucv * pcue1 * pcve2;
    pcue1sve2 = scusv * pcue1 * psve2;
    spsue1 = ssu * psue1;
    spcue1 = scu * pcue1;
    spsue11 = ssu * psue11;
    pb13 = pi * b1 * b3;
    pb123 = pi * b1 * b2 * b3;
    comb = pi * b3 * (ssu * psue1 + b2 / sa3);
    scomb = sin(comb); ccomb = cos(comb);
    logcu = (fabs(cu) > 0.0) ? log(fabs(cu)) : 0.0; 
    logsu = (fabs(su) > 0.0) ? log(fabs(su)) : 0.0;
    logcv = (fabs(cv) > 0.0) ? log(fabs(cv)) : 0.0; 
    logsv = (fabs(sv) > 0.0) ? log(fabs(sv)) : 0.0;

    fsc +=  forcex * (kxsu * a1 * pcue1cve2 +
		    pb123 * scomb / (sc * sa3)) +
           forcey * kysu * a2 * pcue1sve2 +
	   forcez * a3 * ssu * psue1;
    fa1 += forcex * kxsu * sc * pcue1cve2;
    fa2 += forcey * kysu * sc * pcue1sve2;
    fa3 += forcex * pb123 * scomb / (sa3 * a3) +
           forcez * sc * spsue1;
    fe1 += forcex * (tx * sa1 *  logsu * spsue1 * pcue1cve2 +
		    kxsu * sa1 * logcu * pcue1cve2 -
		    pb13 * logsu * spsue1 * scomb)+
           forcey * (ty * sa2 * logsu * spsue1 * pcue1sve2 +
		    kysu * sa2 * logcu * pcue1sve2) +
           forcez * sa3 * logsu * spsue1;
    fe2 += forcex * kxsu * sa1 * logcv * pcue1cve2 +
           forcey * kysu * sa2 * logsv * pcue1sve2;
    ftx += 1000 * forcex * spsue1 * sc * a1 * pcue1cve2;
    fty += 1000 * forcey * spsue1 * sc * a2 * pcue1sve2;
    fb1 += forcex * ccomb;
    fb2 += -forcex * pb13 * scomb / sa3;
    fb3 += -forcex * pi * b1 * (spsue1 + b2 / sa3) * scomb;    
  }


/* here I update current derivatives of superquadric parameters */
  h->dsc = fsc; h->da1 = fa1; h->da2 = fa2; h->da3 = fa3; 
  h->de1 = fe1; h->de2 = fe2; h->dtapx = ftx; h->dtapy = fty;
  h->dbend1 = fb1; h->dbend2 = fb2; h->dbend3 = fb3;


  h->sc += ddt * fsc;
  h->sc = Limit(h->sc, 1.5, 35.0);
  h->a1 += ddt * fa1;
  h->a1 = Limit(h->a1, 0.05, 1.0);
  h->a2 += ddt * fa2;
  h->a2 = Limit(h->a2, 0.05, 1.0);
  h->a3 += ddt * fa3;
  h->a3 = Limit(h->a3, 0.05, 1.0);
  h->e1 += ddt * fe1;
  h->e1 = Limit(h->e1, 1.0, 1.0);
  h->e2 += ddt * fe2;
  h->e2 = Limit(h->e2, 1.0, 1.0);
  h->tapx += ddt * ftx;
  h->tapx = Limit(h->tapx, 0.2, 0.5);
  h->tapy += ddt * fty;
  h->tapy = Limit(h->tapy, 0.2, 0.5);
  h->bend1 += ddt * fb1;
  h->bend1 = Limit(h->bend1, -0.5, 0.7);
  h->bend2 += ddt * fb2;
  h->bend2 = Limit(h->bend2, -1.0, 1.0);
  h->bend3 += ddt * fb3;
  h->bend3 = Limit(h->bend3, 0.3, 0.7);
/*
  printf("*****sc = %f, a1 = %f, a2 = %f, a3 = %f, e1 = %f, e2 = %f, tapx = %f, tapy = %f, bend1 = %f, bend2 = %f, bend3 = %f****\n",h->sc, h->a1, h->a2, h->a3, h->e1, h->e2, h->tapx, h->tapy, h->bend1, h->bend2, h->bend3);
*/
/*
  h->a3 = Limit(h->a3, 0.00001, 1.0);
      printf("***forcex = %f, pi = %f, b1 = %f, b3 = %f, scomb = %f\n", forcex,pi, b1, b3, scomb);

  printf("*****sc = %f, a1 = %f, a2 = %f, a3 = %f, e1 = %f, e2 = %f, tapx = %f, tapy = %f, bend1 = %f, bend2 = %f, bend3 = %f****\n",h->sc, h->a1, h->a2, h->a3, h->e1, h->e2, h->tapx, h->tapy, h->bend1, h->bend2, h->bend3);
*/
  compute_superquadric_taper_bend(h);

}



void adapt_superquadric_derivatives_taper_bend(hybroid *h, int pos, vector fc, 
                                                         double *ider)
{
  node *ndei, *nds = h->nodes;
  double forcex, forcey, forcez;

  double sc = h->sc, a1 = h->a1, a2 = h->a2, a3 = h->a3, 
         e1 = h->e1, e2 = h->e2, tx = h->tapx, ty = h->tapy,
         b1 = h->bend1, b2 = h->bend2, b3 = h->bend3,
         sa1 = sc * a1, sa2 = sc * a2, 
         sa3 = sc * a3, a1e1 = sc * a1 * e1, a1e2 = sc * a1 * e2,
         a2e1 = sc * a2 * e1, a2e2 = sc * a2 * e2, a3e1 = sc * a3 * e1;

  double u, v, uc, vc, cu, cv, su, sv, scucv, scusv, ssu, scu;
  double pcue1, pcue11, pcve21, pcve2, psue1, psve2, psue11, psve21,
         pcue1cve2, pcue1sve2, spsue1, spcue1, spsue11, kxsu, kysu, 
         pb13, pb123, comb, scomb, ccomb, logsu, logsv, logcu, logcv;
  double pi = 3.14159265358979323;
         
  ndei = &nds[pos];
  cu = cos(ndei->uu); su = sin(ndei->uu);
  cv = cos(ndei->vv); sv = sin(ndei->vv);
  forcex = fc.x;
  forcey = fc.y;
  forcez = fc.z;

  scucv = Sign(cu * cv); scusv = Sign(cu * sv); ssu = Sign(su);
  scu = Sign(cu);

  pcue1 = (cu < 0.0 || cu > 0.0) ? pow(fabs(cu), e1) : 0.0;
  pcue11 = (cu < 0.0 || cu > 0.0) ? pow(fabs(cu), e1 - 1.0) : 0.0;
  pcve2 = (cv < 0.0 || cv > 0.0) ? pow(fabs(cv), e2) : 0.0;
  pcve21 = (cv < 0.0 || cv > 0.0) ? pow(fabs(cv), e2 - 1.0) : 0.0;
  psve2 = (sv < 0.0 || sv > 0.0) ? pow(fabs(sv), e2) : 0.0;
  psve21 = (sv < 0.0 || sv > 0.0) ? pow(fabs(sv), e2 - 1.0) : 0.0;
  psue1 = (su < 0.0 || su > 0.0) ? pow(fabs(su), e1) : 0.0;
  psue11 = (su < 0.0 || su > 0.0) ? pow(fabs(su), e1 - 1.0) : 0.0;
  
  kxsu = tx * ssu * psue1 + 1.0;
  kysu = ty * ssu * psue1 + 1.0;
  pcue1cve2 = scucv * pcue1 * pcve2;
  pcue1sve2 = scusv * pcue1 * psve2;
  spcue1 = scu * pcue1;
  spsue1 = ssu * psue1;
  spsue11 = ssu * psue11;
  pb13 = pi * b1 * b3;
  pb123 = pi * b1 * b2 * b3;
  comb = pi * b3 * (ssu * psue1 + b2 / sa3);
  scomb = sin(comb); ccomb = cos(comb);
  logcu = (fabs(cu) > 0.0) ? log(fabs(cu)) : 0.0; 
  logsu = (fabs(su) > 0.0) ? log(fabs(su)) : 0.0;
  logcv = (fabs(cv) > 0.0) ? log(fabs(cv)) : 0.0; 
  logsv = (fabs(sv) > 0.0) ? log(fabs(sv)) : 0.0;

  
  ider[0] = forcex * (kxsu * a1 * pcue1cve2 +
		      pb123 * scomb / (sc * sa3)) +
            forcey * kysu * a2 * pcue1sve2 +
	    forcez * a3 * ssu * psue1;
  ider[1] = forcex * kxsu * sc * pcue1cve2;
  ider[2] = forcey * kysu * sc * pcue1sve2;
  ider[3] = forcex * pb123 * scomb / (sa3 * a3) +
            forcez * sc * spsue1;
  ider[4] = forcex * (tx * sa1 *  logsu * spsue1 * pcue1cve2 +
		      kxsu * sa1 * logcu * pcue1cve2 -
		      pb13 * logsu * spsue1 * scomb)+
            forcey * (ty * sa2 * logsu * spsue1 * pcue1sve2 +
		      kysu * sa2 * logcu * pcue1sve2) +
            forcez * sa3 * logsu * spsue1;
  ider[5] = forcex * kxsu * sa1 * logcv * pcue1cve2 +
            forcey * kysu * sa2 * logsv * pcue1sve2;
  ider[6] = forcex * spsue1 * sc * a1 * pcue1cve2;
  ider[7] = forcey * spsue1 * sc * a2 * pcue1sve2;
  ider[8] = forcex * ccomb;
  ider[9] = -forcex * pb13 * scomb / sa3;
  ider[10] = -forcex * pi * b1 * (spsue1 + b2 / sa3) * scomb;    



/* here I increment current derivatives of superquadric parameters */
  h->dsc += ider[0]; h->da1 += ider[1]; h->da2 += ider[2]; h->da3 += ider[3]; 
  h->de1 += ider[4]; h->de2 += ider[5]; h->dtapx += ider[6]; 
  h->dtapy += ider[7]; h->dbend1 += ider[8]; h->dbend2 += ider[9]; 
  h->dbend3 += ider[10];

}



void decrement_superquadric_derivatives_taper_bend(hybroid *h, double *ider)
{
  h->dsc -= ider[0]; h->da1 -= ider[1]; h->da2 -= ider[2]; h->da3 -= ider[3]; 
  h->de1 -= ider[4]; h->de2 -= ider[5]; h->dtapx -= ider[6]; 
  h->dtapy -= ider[7]; h->dbend1 -= ider[8]; h->dbend2 -= ider[9]; 
  h->dbend3 -= ider[10];
}



void compute_superquadric_derivative_taper_bend(hybroid *h, int pos, 
                                                      vector *sdot)
{
  node *ndei, *nds = h->nodes;

  double sc = h->sc, a1 = h->a1, a2 = h->a2, a3 = h->a3, 
         e1 = h->e1, e2 = h->e2, tx = h->tapx, ty = h->tapy,
         b1 = h->bend1, b2 = h->bend2, b3 = h->bend3,
         sa1 = sc * a1, sa2 = sc * a2,
         sa3 = sc * a3, a1e1 = sc * a1 * e1, a1e2 = sc * a1 * e2,
         a2e1 = sc * a2 * e1, a2e2 = sc * a2 * e2, a3e1 = sc * a3 * e1;

  double dsc = h->dsc, da1 = h->da1, da2 = h->da2, da3 = h->da3,
         de1 = h->de1, de2 = h->de2, dtx = h->dtapx, dty = h->dtapy,
         db1 = h->dbend1, db2 = h->dbend2, db3 = h->dbend3;

  double u, v, uc, vc, cu, cv, su, sv, scucv, scusv, ssu, scu;
  double pcue1, pcue11, pcve21, pcve2, psue1, psve2, psue11, psve21,
         pcue1cve2, pcue1sve2, spsue1, spcue1, spsue11, kxsu, kysu, 
         pb13, pb123, comb, scomb, ccomb, logcu, logsu, logsv, logcv;
  double pi = 3.14159265358979323;
         
  ndei = &nds[pos];
  cu = cos(ndei->uu); su = sin(ndei->uu);
  cv = cos(ndei->vv); sv = sin(ndei->vv);
  scucv = Sign(cu * cv); scusv = Sign(cu * sv); ssu = Sign(su);
  scu = Sign(cu);

  pcue1 = (cu < 0.0 || cu > 0.0) ? pow(fabs(cu), e1) : 0.0;
  pcue11 = (cu < 0.0 || cu > 0.0) ? pow(fabs(cu), e1 - 1.0) : 0.0;
  pcve2 = (cv < 0.0 || cv > 0.0) ? pow(fabs(cv), e2) : 0.0;
  pcve21 = (cv < 0.0 || cv > 0.0) ? pow(fabs(cv), e2 - 1.0) : 0.0;
  psve2 = (sv < 0.0 || sv > 0.0) ? pow(fabs(sv), e2) : 0.0;
  psve21 = (sv < 0.0 || sv > 0.0) ? pow(fabs(sv), e2 - 1.0) : 0.0;
  psue1 = (su < 0.0 || su > 0.0) ? pow(fabs(su), e1) : 0.0;
  psue11 = (su < 0.0 || su > 0.0) ? pow(fabs(su), e1 - 1.0) : 0.0;
  
  kxsu = tx * ssu * psue1 + 1.0;
  kysu = ty * ssu * psue1 + 1.0;
  pcue1cve2 = scucv * pcue1 * pcve2;
  pcue1sve2 = scusv * pcue1 * psve2;
  spcue1 = scu * pcue1;
  spsue1 = ssu * psue1;
  spsue11 = ssu * psue11;
  pb13 = pi * b1 * b3;
  pb123 = pi * b1 * b2 * b3;
  comb = pi * b3 * (ssu * psue1 + b2 / sa3);
  scomb = sin(comb); ccomb = cos(comb);
  logcu = (fabs(cu) > 0.0) ? log(fabs(cu)) : 0.0; 
  logsu = (fabs(su) > 0.0) ? log(fabs(su)) : 0.0;
  logcv = (fabs(cv) > 0.0) ? log(fabs(cv)) : 0.0; 
  logsv = (fabs(sv) > 0.0) ? log(fabs(sv)) : 0.0;

  
  sdot->x = (kxsu * a1 * pcue1cve2 + pb123 * scomb / (sc * sa3)) * dsc +
             kxsu * sc * pcue1cve2 * da1 + 
             pb123 * scomb / (sa3 * a3) * da3 +
	    (tx * sa1 *  logsu * spsue1 * pcue1cve2 +
             kxsu * sa1 * logcu * pcue1cve2 -
	     pb13 * logsu * spsue1 * scomb) * de1 +
            kxsu * sa1 * logcv * pcue1cve2 * de2 +
            spsue1 * sc * a1 * pcue1cve2 * dtx +
	    ccomb * db1 - pb13 * scomb / sa3 * db2 -
	    pi * b1 * (spsue1 + b2 / sa3) * scomb * db3;

  sdot->y = kysu * a2 * pcue1sve2 * dsc +
            kysu * sc * pcue1sve2 * da2 +
            (ty * sa2 * logsu * spsue1 * pcue1sve2 +
	     kysu * sa2 * logcu * pcue1sve2) * de1 +
            kysu * sa2 * logsv * pcue1sve2 * de2 +
            spsue1 * sc * a2 * pcue1sve2 * dty;

  sdot->z = a3 * ssu * psue1 * dsc +
            sc * spsue1 * da3 +
            sa3 * logsu * spsue1 * de1;  
}



